
#ifndef motc
#define motc

# include <rrcon.h>

# define MOTOR1 0
# define MOTOR2 3

# define MAXSPEED 1023 // Maximum speed of the motors

using namespace std;
using namespace kthrobot;

class motors_control : public XTask {

public:

  Motor& right_motor;
  Motor& left_motor;
  EncoderData ticks1;
  EncoderData ticks2;

  int motor_speed1;
  int motor_speed2;

  const static double mm_per_tick = 0.115384615384615;
  const static int ticks_per_wheelrev = 624;
  const static double wheel_radius = 72;

  motors_control() : XTask("motors_control"), right_motor(RRCon::motor(MOTOR1)), left_motor(RRCon::motor(MOTOR2)) {
    SetPeriodMS(1000);
    motor_speed1 = 900;
    motor_speed2 = 900;
    right_motor.SetEncToZero();
    left_motor.SetEncToZero();
    right_motor.SetSpeed(motor_speed1);
    left_motor.SetSpeed(motor_speed2);
  }

  void TaskCode() {

    int tick_speed1 = 0;
    int tick_speed2 = 0;

      while( !MustStop() ) {
        ticks1 = right_motor.ReadEnc();
        ticks2 = left_motor.ReadEnc();
        tick_speed1 = ticks1.cur - ticks1.prev;
        tick_speed2 = ticks2.cur - ticks2.prev;

        double sp1 = tick_speed1 * mm_per_tick / 10;
        double sp2 = tick_speed2 * mm_per_tick / 10;

        Log::Write( "Motor 1 speed: %f cm/sec", sp1 );
        Log::Write( "Motor 2 speed: %f cm/sec", sp2 );

        WaitPeriod();
      }
  }

  ~motors_control() {
    Join();
  }

  void turnMotorsOff() {
    motor_speed1 = 0;
    motor_speed2 = 0;
    right_motor.SetSpeed(0);
    left_motor.SetSpeed(0);
  }

  void setSpeeds( int m1, int m2 ) {
    motor_speed1 = m1;
    motor_speed2 = m2;
    right_motor.SetSpeed(m1);
    left_motor.SetSpeed(m2);
  }

  void setSpeed( int motor, int speed ) {
    if ( motor == 1 ) {
      motor_speed1 = speed;
      right_motor.SetSpeed(speed);
    }
    else if ( motor == 2 ) {
      motor_speed2 = speed;
      left_motor.SetSpeed(speed);
    }
  }

};

#endif
